{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.spatial import KDTree\n",
    "class ICP:\n",
    "    \"\"\" \n",
    "    ICP（Iterative Closest Point，迭代最近点）算法是一种用于计算两个点云之间变换矩阵的算法。以下是ICP算法的实现流程：\n",
    "\n",
    "    初始化变换矩阵为单位矩阵。\n",
    "    在当前变换矩阵下，将源点云进行变换，得到变换后的源点云。\n",
    "    计算变换后的源点云和目标点云之间的最近点对。\n",
    "    计算源点云和目标点云之间的变换矩阵。\n",
    "    更新变换矩阵。\n",
    "    判断是否满足终止条件，如果满足则结束迭代，否则返回步骤2。\n",
    "    \"\"\"\n",
    "    def __init__(self, max_iterations=100, tolerance=1e-6):\n",
    "        self.max_iterations = max_iterations\n",
    "        self.tolerance = tolerance\n",
    "        self.transformation = np.eye(4)\n",
    "    def fit(self, source, target):\n",
    "            for i in range(self.max_iterations):\n",
    "                # 计算当前变换矩阵下的源点云\n",
    "                source_transformed = self.transform(source)\n",
    "                # 计算源点云和目标点云之间的最近点对\n",
    "                distances, indices = self.nearest_neighbor(source_transformed, target)\n",
    "                # 计算源点云和目标点云之间的变换矩阵\n",
    "                transformation = self.calculate_transformation(source_transformed, target[indices])\n",
    "                # 更新变换矩阵\n",
    "                self.transformation = np.dot(transformation, self.transformation)\n",
    "                # 判断是否满足终止条件\n",
    "                if np.linalg.norm(transformation - np.eye(4)) < self.tolerance:\n",
    "                    break\n",
    "            return self.transformation\n",
    "    def nearest_neighbor(self, source, target):\n",
    "            tree = KDTree(target)\n",
    "            distances, indices = tree.query(source)\n",
    "            return distances, indices\n",
    "    def calculate_transformation(self, source, target):\n",
    "            # 计算源点云和目标点云之间的质心\n",
    "            source_mean = np.mean(source, axis=0)\n",
    "            target_mean = np.mean(target, axis=0)\n",
    "            # 计算源点云和目标点云之间的协方差矩阵\n",
    "            # source - source_mean 每个点到质心的差值\n",
    "            covariance = np.dot((source - source_mean).T, target - target_mean) / len(source)\n",
    "            # 计算变换矩阵\n",
    "            U, _, Vt = np.linalg.svd(covariance)\n",
    "            R = np.dot(Vt.T, U.T)\n",
    "            t = target_mean - np.dot(R, source_mean)\n",
    "            transformation = np.eye(4)\n",
    "            transformation[:3, :3] = R\n",
    "            transformation[:3, 3] = t\n",
    "            return transformation\n",
    "    def transform(self, points):\n",
    "            return np.dot(self.transformation[:3, :3], points.T).T + self.transformation[:3, 3]\n",
    "\n",
    "\n",
    "source = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]])\n",
    "target = np.array([[1, 1, 1], [2, 1, 1], [1, 2, 1], [1, 1, 2]])\n",
    "icp = ICP()\n",
    "transformation = icp.fit(source, target)\n",
    "print(transformation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "协方差矩阵是一种用于衡量两个随机变量之间关系的矩阵。在ICP算法中，协方差矩阵用于计算源点云和目标点云之间的变换矩阵。\n",
    "\n",
    "下面是计算协方差矩阵的步骤：\n",
    "\n",
    "1. **计算源点云和目标点云的质心**：\n",
    "   ```Python\n",
    "   source_mean = np.mean(source, axis=0)\n",
    "   target_mean = np.mean(target, axis=0)\n",
    "   ```\n",
    "   质心是点云中所有点的平均值。在这个例子中，`source_mean`和`target_mean`分别是源点云和目标点云的质心。\n",
    "\n",
    "2. **计算源点云和目标点云之间的差值**：\n",
    "   ```Python\n",
    "   source_diff = source - source_mean\n",
    "   target_diff = target - target_mean\n",
    "   ```\n",
    "   `source_diff`和`target_diff`分别是源点云和目标点云中每个点到其质心的差值。\n",
    "\n",
    "3. **计算协方差矩阵**：\n",
    "   ```Python\n",
    "   covariance = np.dot(source_diff.T, target_diff) / len(source)\n",
    "   ```\n",
    "   协方差矩阵是源点云和目标点云差值的协方差矩阵。协方差矩阵的元素`covariance[i, j]`表示源点云中第`i`个维度和目标点云中第`j`个维度的协方差。\n",
    "   表示每个点间的系数，两两相乘得到相关系数\n",
    "\n",
    "在ICP算法中，协方差矩阵用于计算源点云和目标点云之间的变换矩阵。具体来说，协方差矩阵的奇异值分解（SVD）可以用于计算旋转矩阵和平移向量，从而得到源点云到目标点云的变换矩阵。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "协方差矩阵是一种用于衡量两个随机变量之间关系的矩阵。在ICP算法中，协方差矩阵用于计算源点云和目标点云之间的变换矩阵。\n",
    "\n",
    "下面是计算协方差矩阵的步骤：\n",
    "\n",
    "1. **计算源点云和目标点云的质心**：\n",
    "   ```Python\n",
    "   source_mean = np.mean(source, axis=0)\n",
    "   target_mean = np.mean(target, axis=0)\n",
    "   ```\n",
    "   质心是点云中所有点的平均值。在这个例子中，`source_mean`和`target_mean`分别是源点云和目标点云的质心。\n",
    "\n",
    "2. **计算源点云和目标点云之间的差值**：\n",
    "   ```Python\n",
    "   source_diff = source - source_mean\n",
    "   target_diff = target - target_mean\n",
    "   ```\n",
    "   `source_diff`和`target_diff`分别是源点云和目标点云中每个点到其质心的差值。\n",
    "\n",
    "3. **计算协方差矩阵**：\n",
    "   ```Python\n",
    "   covariance = np.dot(source_diff.T, target_diff) / len(source)\n",
    "   ```\n",
    "   协方差矩阵是源点云和目标点云差值的协方差矩阵。协方差矩阵的元素`covariance[i, j]`表示源点云中第`i`个维度和目标点云中第`j`个维度的协方差。\n",
    "\n",
    "在ICP算法中，协方差矩阵用于计算源点云和目标点云之间的变换矩阵。具体来说，协方差矩阵的奇异值分解（SVD）可以用于计算旋转矩阵和平移向量，从而得到源点云到目标点云的变换矩阵。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "是的，奇异值分解（SVD）可以用于计算旋转矩阵和平移矩阵，这是一个近似逼近的过程。\n",
    "\n",
    "在ICP算法中，协方差矩阵的奇异值分解（SVD）用于计算源点云和目标点云之间的变换矩阵。具体来说，协方差矩阵的奇异值分解可以分解为以下形式：\n",
    "\n",
    "```\n",
    "C = U * S * V^T\n",
    "```\n",
    "\n",
    "其中，`U`和`V`是正交矩阵，`S`是对角矩阵，对角线上的元素是协方差矩阵的奇异值。\n",
    "\n",
    "在ICP算法中，我们通常假设源点云和目标点云之间的变换只包含旋转和平移，而不包含缩放。因此，我们可以假设`S`是一个对角矩阵，其对角线上的元素都是1。这样，我们可以得到：\n",
    "\n",
    "```\n",
    "C = U * V^T\n",
    "```\n",
    "\n",
    "然后，我们可以通过以下方式计算旋转矩阵`R`和平移向量`t`：\n",
    "\n",
    "```\n",
    "R = U * V^T\n",
    "t = target_mean - R * source_mean\n",
    "```\n",
    "\n",
    "其中，`R`是旋转矩阵，`t`是平移向量。\n",
    "\n",
    "需要注意的是，这个方法是一个近似逼近的过程，因为它假设了源点云和目标点云之间的变换只包含旋转和平移，而不包含缩放。在实际应用中，可能需要使用更复杂的方法来处理缩放和其他类型的变换。"
   ]
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
